#include <doors/udp.h>

#include "coapconstants.h"
#include "coappdu.i"


///////////////////////////////////////////////
// Local functions
//////////////////////////////////////////////

static Uint16 readOptions(Frame& options, Frame& frame, Uint16 optionsCount)
{
	//std::cout << "*************************************************" << std::endl; 
	//std::cout << "Number of options: " << optionsCount << std::endl;
	//std::cout << "*************************************************" << std::endl; 

	Uint16 jj = 4;
	for (Uint8 ii = 0; ii < optionsCount; ii++) {
    	Uint8 optionLength = (frame.read(jj) & 0xf0) >> 4;
    	options.putLast(frame.read(jj++));
    	if (optionLength == 0xf) {
 			optionLength = static_cast<Uint16>(frame.read(jj++)) + 0xf;
			options.putLast(frame.read(jj++));
		}
		//std::cout << "Option length: " << std::hex << static_cast<int>(optionLength) << std::endl;
		for (Uint16 kk = 0; kk < optionLength; kk++) {
			options.putLast(frame.read(jj++));
		}
	}

	return jj;
}

///////////////////////////////////////////////
// Class methods
//////////////////////////////////////////////

Uint16
CoapPeer::CON::decode (Message* msg)
{
  // UdpSAP::Dtind provides the whole UDP PDU in its 'data' attribute
  Frame frame = ((UdpSAP::Dtind *) msg)->data;

  // Set address to the source address
  addr = ((UdpSAP::Dtind *) msg)->source;

  //////////////////
  // Header 4 bytes
  //////////////////
  // First byte
  header0 = frame.read(0);
  // Second byte
  code = frame.read(1);
  // Third & fourth bytes
  msgID = (frame.read(3) << 8) 
     | frame.read(2);

  std::cout << "*************************************************" << std::endl; 
  std::cout << "*** CON DECODE" << std::endl; 
  std::cout << "Header0: " << std::hex << static_cast<Uint16>(header0) << std::endl;
  std::cout << "Code: " << std::hex << static_cast<Uint16>(code) << std::endl;
  std::cout << "Message ID: " << std::hex << static_cast<Uint16>(msgID) << std::endl;

  //////////////////
  // Options
  //////////////////
  Uint16 optionsCount = static_cast<Uint16>((header0 & 0xf0) >> 4);
  Uint16 bytesIter = readOptions(options, frame, optionsCount);

  //////////////////
  // Payload
  //////////////////
  FrameSize payloadSize = frame.length();
  std::cout << "Datagram size: " << payloadSize << std::endl;
  payloadSize -= bytesIter;
  std::cout << "Payload size: " << payloadSize << std::endl;

  for (Uint16 ii = 0; ii < payloadSize; ++ii)
    payload.putLast(frame.read(bytesIter++));


  return 1; // Return number of bytes decoded from dtind payload.
}

Message*
CoapPeer::CON::encode (void)
{
  // Create a new udp packet to dtreq message
  UdpSAP::Dtreq *dtreq = new UdpSAP::Dtreq;

  // Set the destination address
  dtreq->dest = addr;

  std::cout << "*************************************************" << std::endl; 
  std::cout << "CON ENCODE" << std::endl;  
  std::cout << "Server address: " << addr.getHostName() << " " << addr.getHostAddr() << std::endl;

  //////////////////
  // Header 4 bytes
  //////////////////
  dtreq->data.putLast(header0);
  dtreq->data.putLast(code);
  // dtreq->data.putLast((const Byte*) &messageID, 2);
  dtreq->data.putLast(msgID & 0x00ff);
  dtreq->data.putLast((msgID & 0xff00) >> 8); 
  std::cout << "Header: " << 
     std::hex << static_cast<Uint16>(dtreq->data.read(3)) << " "  <<
     std::hex << static_cast<Uint16>(dtreq->data.read(2)) << " "  <<
     std::hex << static_cast<Uint16>(dtreq->data.read(1)) << " "  <<
     std::hex << static_cast<Uint16>(dtreq->data.read(0)) << " "  << std::endl;

  //////////////////
  // Options
  //////////////////
  Uint16 opLength = options.length();
  std::cout << "Number of options: " << opLength / 2 << std::endl;
  for (int ii = 0; ii < opLength; ii++) {
     dtreq->data.putLast(options.getFirst());

     // Debugging 
     Uint16 opCounter = 1;
     if (ii % 2) {

	std::cout << "Option " << opCounter++ << ": " 
		  << std::hex << static_cast<Uint16>(dtreq->data.read(4 + ii))
		  << " " 
		  << std::hex << static_cast<Uint16>(dtreq->data.read(4 + ii - 1))
		  << std::endl;
     }
  }

  //////////////////
  // Payload
  //////////////////
  std::cout << "Payload size: " << payload.length() << std::endl;
  dtreq->data.combine(payload); 
     
  std::cout << "*************************************************" << std::endl;      
  return dtreq;
}

Uint16
CoapPeer::ACK::decode (Message* msg)
{
   std::cout << "***************RECEIVED ACK***********************" << std::endl; 

  // UdpSAP::Dtind provides the whole UDP PDU in its 'data' attribute
  Frame frame = ((UdpSAP::Dtind *) msg)->data;

  // Set address to the source address
  addr = ((UdpSAP::Dtind *) msg)->source;

  //////////////////
  // Header 4 bytes
  //////////////////
  // First byte
  header0 = frame.read(0);
  // Second byte
  code = frame.read(1);
  // Third & fourth bytes
  msgID = (frame.read(3) << 8) 
     | frame.read(2);

  std::cout << "*************************************************" << std::endl; 
  std::cout << "ACK DECODE" << std::endl; 
  std::cout << "Header0: " << std::hex << static_cast<Uint16>(header0) << std::endl;
  std::cout << "Code: " << std::hex << static_cast<Uint16>(code) << std::endl;
  std::cout << "Message ID: " << std::hex << static_cast<Uint16>(msgID) << std::endl;

  //////////////////
  // Options
  //////////////////
  Uint16 optionsCount = static_cast<Uint16>((header0 & 0xf0) >> 4);
  Uint16 bytesIter = readOptions(options, frame, optionsCount);

  //////////////////
  // Payload
  //////////////////
  FrameSize payloadSize = frame.length();
  std::cout << "Datagram size: " << payloadSize << std::endl;

  payloadSize -= bytesIter;
  std::cout << "Payload size: " << payloadSize << std::endl;

  for (Uint16 ii = 0; ii < payloadSize; ++ii)
     payload.putLast(frame.read(bytesIter++));

  return 1;
}

Message*
CoapPeer::ACK::encode (void)
{
  // Create a new udp packet to dtreq message
  UdpSAP::Dtreq *dtreq = new UdpSAP::Dtreq;

  // Set the destination address
  dtreq->dest = addr;

  std::cout << "*************************************************" << std::endl; 
  std::cout << "ACK ENCODE" << std::endl; 
  std::cout << "Client address: " << addr.getHostName() << " " << addr.getHostAddr() << std::endl;
  
  //////////////////
  // Header 4 bytes
  //////////////////
  dtreq->data.putLast(header0);
  dtreq->data.putLast(code);
  // dtreq->data.putLast((const Byte*) &messageID, 2);
  dtreq->data.putLast(msgID & 0x00ff);
  dtreq->data.putLast((msgID & 0xff00) >> 8); 
  std::cout << "Header: " << 
     std::hex << static_cast<Uint16>(dtreq->data.read(3)) << " "  <<
     std::hex << static_cast<Uint16>(dtreq->data.read(2)) << " "  <<
     std::hex << static_cast<Uint16>(dtreq->data.read(1)) << " "  <<
     std::hex << static_cast<Uint16>(dtreq->data.read(0)) << " "  << std::endl;

  //////////////////
  // Options
  //////////////////
  Uint16 opLength = options.length();
  //std::cout << "Number of options: " << opLength / 2 << std::endl;
  for (int ii = 0; ii < opLength; ii++) {
     dtreq->data.putLast(options.getFirst());

     // Debugging 
     Uint16 opCounter = 1;
     if (ii % 2) {
		  std::cout << "Option " << opCounter++ << ": " 
		  << std::hex << static_cast<Uint16>(dtreq->data.read(4 + ii))
		  << " " 
		  << std::hex << static_cast<Uint16>(dtreq->data.read(4 + ii - 1))
		  << std::endl;
     }
  }

  //////////////////
  // Payload
  //////////////////
  std::cout << "Payload size: " << payload.length() << std::endl;
  dtreq->data.combine(payload);

  std::cout << "*************************************************" << std::endl; 
  return dtreq;
}

Uint16
CoapPeer::NON::decode (Message* msg)
{
   return 1;
}

Message*
CoapPeer::NON::encode (void)
{
  // Create a new udp packet to dtreq message
  UdpSAP::Dtreq *dtreq = new UdpSAP::Dtreq;

  return dtreq;
}


Uint16
CoapPeer::RST::decode (Message* msg)
{
   return 1;
}

Message*
CoapPeer::RST::encode (void)
{
  // Create a new udp packet to dtreq message
  UdpSAP::Dtreq *dtreq = new UdpSAP::Dtreq;

  return dtreq;
}


// Identifies the received PDU
Sint16 
CoapPeer::identify (Message *msg) 
{
   // Return the PDU type based on the 
   // Type field in the first octed of the header
   Uint16 msgType = (((UdpSAP::Dtind *) msg)->data.read(0) & 0xC) >> 2;
   switch (msgType) {
      case MSG_CON :
	 return con;
      case MSG_NON :
	 return non;
      case MSG_ACK:
	 return ack;
      case MSG_RST:
	 return rst;
   }
   
   return other;
}

